#!/bin/bash

# 定义存储进程名和PID的数组（格式：进程名:pid）
declare -a PROC_ENTRIES

cleanup() {
    echo -e "\nStopping all nodes..."
    for entry in "${PROC_ENTRIES[@]}"; do
        # 分割进程名和PID
        proc_name="${entry%:*}"
        pid="${entry#*:}"
        
        # 终止进程并显示信息
        if kill $pid 2>/dev/null; then
            echo "已终止进程 '$proc_name' (PID: $pid)"
        else
            echo "无法终止进程 '$proc_name' (PID: $pid)"
        fi
    done
    killall monitor 
    exit 0
}

trap cleanup INT TERM EXIT

# 清理旧日志
rm log/*.* -rf

# 启动函数：记录进程名和PID
launch_node() {
    local node_name=$1
    shift
    echo "Bring up $node_name..."
    "$@" > /dev/null 2>&1 &
    PROC_ENTRIES+=("${node_name}:$!")
}

# 启动所有节点
launch_node "process_monitor" ros2 run agv_bringup process_monitor
launch_node "temp_monitor" ros2 run agv_bringup temp_mointor
launch_node "shared_parameters" ros2 run shared_parameters param_server
launch_node "record" ros2 run record record_node
launch_node "kacanopen" ros2 run kacanopen kacanopen_apps_kacanopen_new --ros-args -r __node:=kacanopen
sleep 1

launch_node "lidar_driver" ros2 run pf_driver ros_main --ros-args \
    --params-file $(ros2 pkg prefix pf_driver)/share/pf_driver/config/R2000_Default_params.yaml \
    --remap __node:=pf_r2000

launch_node "tf_laser" ros2 run tf_laser laser_transform_publisher     
launch_node "modbus" ros2 run modbus master_node
launch_node "io_module" ros2 run io_module agv_set_io
launch_node "controller_manager" ros2 run controller_manager ros2_control_node
launch_node "agv_peripheral_controller" ros2 run agv_peripheral_controller agv_peripheral_controller_node

sleep 1
launch_node "twist_mux" ros2 run twist_mux twist_mux --ros-args \
    --remap cmd_vel_out:=/cmd_vel \
    --params-file $(ros2 pkg prefix agv_bringup)/share/agv_bringup/config/default_twist_mux_locks.yaml \
    --params-file $(ros2 pkg prefix agv_bringup)/share/agv_bringup/config/default_twist_mux_topics.yaml
    
launch_node "code_read_camera" ros2 run code_read_camera code_read_camera_node
sleep 1
launch_node "qr_odom" ros2 run qrodom qrodom
sleep 1
launch_node "agv_navigate" ros2 run agv_navigate agv_navigate --ros-args --remap /cmd_vel:=/twist_mux/nav_vel 
sleep 1
launch_node "agv_dispatch" ros2 run agv_dispatch_comm agv_dispatch_comm_node --ros-args -r __node:=agv_dispatch
sleep 1
launch_node "monitor" ros2 run state_monitor monitor
launch_node "indicators" ros2 run agv_indicators agv_indicators
sleep 1
launch_node "tcp_bridge" ros2 run agv_tcp_bridge tcp_bridge
launch_node "agv_obstacle_avoidance" ros2 run agv_obstacle_avoidance agv_obstacle_avoidance
launch_node "pubulish_map" ros2 run slam_ros publish_occupancy_grid_map
launch_node "edit_map" ros2 run slam_ros edit_map
launch_node "slam_ros" ros2 run slam_ros slam_ros_node
launch_node "agv_teleop" ros2 run agv_teleop agv_teleop_HD1000
launch_node "agv_ftp_log" ros2 run agv_ftp_log log_file_config

echo -e "\nAGV nodes already running. Press Ctrl+C to stop..."
wait
